#include "MiradasWidget.h"
//#include "QArmGeometry.h"
#include "QMiraP3ArmPainter.h"

#include <QDebug>
#include <QFile>
#include <QMatrix>
#include <QPainter>
#include <QPaintEvent>
#include <QString>
#include <QStringList>
#include <QTextStream>

#include <iostream>
#include <cassert>

enum class LoadAnimState { ROBOT_HEADER, ROBOT_ANIM };

MiradasWidget::MiradasWidget(QWidget *parent) :
    QWidget(parent), fileAnimLoaded_(false){
    this->setAttribute(Qt::WA_OpaquePaintEvent,true);
    this->setAttribute(Qt::WA_StaticContents, true);
    this->setAttribute(Qt::WA_NoSystemBackground, true);

    Animator_.setUpdateInterval( 200 );
    Animator_.setCurveShape(QTimeLine::LinearCurve);
    
    connect(&Animator_,SIGNAL(frameChanged(int)),this,SLOT(update()));    
}


MiradasWidget::~MiradasWidget()
{
}

void MiradasWidget::StartAnimation()
{
    Animator_.start();
}

void MiradasWidget::StopAnimation()
{
    Animator_.stop();
}

void MiradasWidget::RewindAnimation()
{
    Animator_.setCurrentTime(0);
}

void MiradasWidget::LoadSceneFile( const QString &fileName ){
    using namespace QMira;
    (void) fileName;
    
    const MiraGeometryLib::Placement2D location(0,0,0);
    const MXSLens dims(1000, 250, 391.59);
    const unsigned int nRobots=9;
    
    /* Init system (smart pointer) */    
    mxsSystemPtr_.reset( new MXS(location, dims, nRobots) );
    
    /* Free previously allocated mem (smart pointers) */
    painterColl_.clear();
    
    /* Init robot painters */
    for (unsigned int i=0; i<nRobots; ++i){
        const IRobot2D& robot = mxsSystemPtr_->getRobot(i);
        painterColl_.push_back(QIRobot2DPainter::QIRobot2DPaintUnPtr_t(new QP3ArmPainter(robot)) );
    }    
}


bool MiradasWidget::LoadAnimationFile(const QString &fileName ){

    this->StopAnimation();

    fileAnimLoaded_ = false;
    QFile File(fileName);
    int maxFrames = 0;
    if( File.exists() && File.open(QIODevice::ReadOnly) ){
        QTextStream InputStream(&File);
        
        LoadAnimState state = LoadAnimState::ROBOT_HEADER;
        
        int robotId=0, nFrames=0, nQ=0, ctrFrame=0;
        while (!InputStream.atEnd()){
            QString line = InputStream.readLine();
            QStringList tokens = line.split(',',QString::SkipEmptyParts);
            if (state == LoadAnimState::ROBOT_HEADER){
                
                if (tokens.size()!=3){
                    qWarning() << "[MIRADAS anim. file]: invalid robot header at line: " << line;
                    return false;                                        
                }
                
                robotId = tokens[0].toUInt();
                nFrames = tokens[1].toUInt();
                nQ = tokens[2].toUInt();
                maxFrames = std::max(nFrames, maxFrames);
                
                ctrFrame = 0;
                state = LoadAnimState::ROBOT_ANIM;
            }
            else {
                ctrFrame++;
                if (tokens.size()!=nQ) {
                    qWarning() << "[MIRADAS anim. file]: invalid configuration from line: " << line;
                    return false;
                }
                
                MiraUtilUBLib::DoubleCont_t qs(nQ);
                for (int i=0; i<nQ; ++i){
                    bool okResult;
                    qs[i] = tokens[i].toFloat(&okResult);
                    if (!okResult){
                        qWarning() << "[MIRADAS anim. file]: invalid configuration from line: " << line;
                        return false;
                    }
                }
                painterColl_[robotId]->addFrame(qs);
                
                if (ctrFrame >= nFrames)
                    state = LoadAnimState::ROBOT_HEADER;
            }
        }

        File.close();

        const int duration = maxFrames - 1;
        Animator_.setDuration(duration * 200);
        Animator_.setFrameRange(0, duration);
        Animator_.setCurrentTime(0);

        fileAnimLoaded_ = true;
    }
    else 
        qDebug() << "[MIRADAS anim. file]: Error opening file '" << fileName << "'";

    return fileAnimLoaded_;
}

QColor MiradasWidget::GetBackgroundColor() const
{
    return BackgroundColor_;
}

void MiradasWidget::SetBackgroundColor(const QColor &value)
{
    BackgroundColor_ = value;
}

void MiradasWidget::SetArmVisibility(const int id, bool visible)
{
}




//////////////////////////////////////////////////////////////////////////////////////
// MIRADAS RENDERING                                                                //
//////////////////////////////////////////////////////////////////////////////////////
void MiradasWidget::paintEvent(QPaintEvent *event){
    (void) event;
    int t = Animator_.currentFrame();
    qDebug() << "Drawing frame " << t;
    drawAll_(t);
}

void MiradasWidget::drawAll_(int t){
    QMatrix m;
    // Translate and Flip the coordinates system to be "cartesian"-like
    m.translate( this->width() / 2 , this->height() / 2 );
    m.scale( 1, -1 );

    QPainter painter(this);
    painter.fillRect(this->rect(), BackgroundColor_);
    painter.setMatrix(m);
    painter.setRenderHint(QPainter::Antialiasing);

    drawAxis_(painter);
    if (fileAnimLoaded_)
        drawArms_(painter, t); 

    painter.end();
}

void MiradasWidget::drawAxis_(QPainter &painter){
    const int MaxX = this->width() / 2;
    const int MaxY = this->height() / 2;

    painter.save();
        QPen Pen(Qt::gray);
        Pen.setStyle(Qt::DashLine);
        painter.setPen(Pen);
        painter.drawLine(QPoint(-(MaxX-5), 0), QPoint(MaxX-5, 0));
        painter.drawLine(QPoint(0, -(MaxY-5)), QPoint(0, MaxY-5));
    painter.restore();
}

void MiradasWidget::drawArms_(QPainter &painter, const int t) {
    
    if (t>0)
        qDebug() << "Drawing frame " << t;
        
    QMira::QIRobot2DPainter::QIRobot2DPaintUnPtrCont_t::iterator it=painterColl_.begin();
    for (;it!=painterColl_.end(); ++it ){
        (*it)->draw(painter, t);
    }    
}
